//
// Created by sukai on 4/1/23.
//

#ifndef CONTNAV_GLOBAL_PLANNER_GLOBAL_PLANNER_H
#define CONTNAV_GLOBAL_PLANNER_GLOBAL_PLANNER_H


#include <ros/ros.h>
#include<ros/package.h>
#include <iostream>
#include <signal.h>
#include <std_msgs/String.h>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
//#include <geometry_msgs/Twist.h>

#include <std_srvs/SetBool.h>

#include <nav_msgs/Path.h>
#include<math.h>

//#include "mbf_msgs/MoveBaseAction.h"
//#include "mbf_msgs/ExePathAction.h"
//#include <actionlib/client/simple_action_client.h>

//路径规划
#include  <contnav_srvs/global_planner.h>
#include <visualization_msgs/Marker.h>
//#include <cmath>
//数据库
//#include "contnav_msgs/Dictionaries.h"
//#include "contnav_msgs/DictionariesVector.h"
//#include "contnav_srvs/TABLE_CONTORLLER.h"
//日志
#include  "contnav_srvs/ContnavLogger.h"
#include  <thread>

//恢复地图
#include <std_srvs/Empty.h>


#include "../common/common.h"
#include  <mutex>

#include "../currencyinclude/potential_calculator.h"
#include "../currencyinclude/expander.h"
#include "../currencyinclude/dijkstra.h"
#include "../currencyinclude/astar.h"
#include "../currencyinclude/orientation_filter.h"
#include <tf2_ros/transform_listener.h>
#include <actionlib/server/simple_action_server.h>
#include <move_base_msgs/MoveBaseAction.h>

#include <dynamic_reconfigure/server.h>
#include <nav_core/base_local_planner.h>
#include <nav_core/base_global_planner.h>
#include <nav_core/recovery_behavior.h>
#include <geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_msgs/GetPlan.h>

#include "auto_lock.h"

//写入文件 writeEulerAnglesToFile
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <fstream>
#include <sys/stat.h>

//路路径优化
#include <algorithm>

#define RED     "\033[31m"      /* Red */
#define GREEN   "\033[32m"      /* Green */
#define WHITE   "\033[37m"      /* White */

//动态参数
#include "contnav_global_planner/ContnavGlobalPlannerConfig.h"
//状态机
#include <functional> // 导入 functional 头文件
#include <unordered_map> // 导入 unordered_map 头文件
#include <string> // 导入 string 头文件
#include <boost/any.hpp> // 导入 boost any 头文件
////用互斥锁保证数据的安全
#include <condition_variable>
/*
condition_variable是C++标准库<condition_variable>中提供的一个类，用于线程间的同步和条件等待。它是多线程编程中常用的同步原语之一，用于协调多个线程的执行。

condition_variable提供了等待和通知机制，可以让一个或多个线程等待某个条件满足，然后再继续执行。它通常与互斥锁（std::mutex）一起使用，以确保在等待条件时不会出现竞争条件。

通过使用condition_variable，一个线程可以等待另一个线程满足某个条件后再继续执行，而不需要进行忙等待或轮询检查。当条件满足时，可以使用notify_one()或notify_all()方法通知等待的线程继续执行。

condition_variable是C++11引入的特性，它提供了更高级的线程同步和通信机制，比较之前的pthread库中的条件变量（pthread_cond_*）更加方便和易用。

要使用condition_variable，需要包含头文件<condition_variable>并创建一个std::condition_variable的实例，然后通过调用其成员函数来等待和通知线程。

例如，std::condition_variable data_cv;创建了一个名为data_cv的condition_variable对象，可以使用wait()和notify_one()等函数来实现线程间的同步和条件等待。
 */

#define contnav_debug(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::DEBUG,className,classLine,format, ##__VA_ARGS__ )

#define contnav_info(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::INFO,className,classLine,format, ##__VA_ARGS__ )
#define contnav_warn(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::WARN,className,classLine,format, ##__VA_ARGS__ )
#define contnav_error(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::ERROR,className,classLine,format, ##__VA_ARGS__ )
#define contnav_fatal(className,classLine,format, ...) \
    saveLogFormat(ContnavLoggerStr::FATAL,className,classLine,format, ##__VA_ARGS__ )

using namespace std;
using namespace contnav::thread;
namespace contnav_global_planner{




//typedefs to help us out with the action server so that we don't hace to type so much
//typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

///using GotoCtrl = actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction>;//点到点
//using ExeCtrl = actionlib::SimpleActionClient<mbf_msgs::ExePathAction>;//给路径直接走


    //用互斥锁保证数据的安全
    std::mutex data_mutex;
    std::condition_variable data_cv;
    bool data_ready = false;

    // 导航状态机类
    class NavigationStateMachine {
    public:
        // 构造函数
        NavigationStateMachine(ros::NodeHandle &nh) : nh_(nh), current_state_("") {}

        //  NavigationStateMachine 类的 add_state 方法以接受参数的向量
        void add_state(const std::string &name, std::function<void(const std::vector<boost::any> &)> state_function) {
            state_functions_[name] = state_function;
        }

        //  NavigationStateMachine 类的 set_state 方法以存储参数向量
        int set_state(const std::string &name, const std::vector<boost::any> &params = {}) {
            current_state_ = name;
            state_params_[name] = params;
            return state_params_[name].size();
        }


        // 获取当前状态的成员函数
        std::string get_current_state() const {
            return current_state_;
        }
        //获取当前状态的参数
        bool get_current_state_param(int index,boost::any &age) const {
            if(index<state_params_.at(current_state_).size()){
                std::vector<boost::any> args = state_params_.at(current_state_);
                age =  args.at(index);
                return true;
            }else{
                return false;
            }
        }

        //  NavigationStateMachine 类的 execute 方法以传递参数向量
        void execute() {
            if (!current_state_.empty() && state_functions_.count(current_state_)) {
                // std::cout << "=======2.开始执行状态机 execute ==================状态机当前状态 " <<current_state_ << std::endl;
                state_functions_[current_state_](state_params_[current_state_]);
            }
        }
        // 增加成员函数用于通知数据准备完毕
        void notifyDataReady() {
            std::lock_guard<std::mutex> lock(data_mutex);
            data_ready = true;
            data_cv.notify_one();
        }
    private:
        ros::NodeHandle nh_; // 节点句柄
        std::string current_state_; // 当前状态的名称
        std::unordered_map<std::string, std::vector<boost::any>> state_params_;//增加多个参数传递,当前状态的参数
        std::unordered_map<std::string, std::function<void(const std::vector<boost::any> &)>> state_functions_; // 存储状态名和对应的函数

    };


    //start-------------求两条线段的交点--------------------------------
    class Point {
    public:
        double x, y;

        Point() {}

        Point(double x, double y) {
            this->x = x;
            this->y = y;
        }
    };

    class Line {
    public:
        double a, b, c;

        Line(Point p1, Point p2) {
            a = p2.y - p1.y;
            b = p1.x - p2.x;
            c = p2.x * p1.y - p1.x * p2.y;
        }

        Point getIntersection(Line other) {
            double det = a * other.b - other.a * b;
            if (det == 0) {
                // lines are parallel or coincident, no intersection 直线平行或重合，无交叉
                return Point();
            }
            double x = (other.b * c - b * other.c) / det;
            double y = (a * other.c - other.a * c) / det;
            return Point(x, y);
        }
    };

    //start-------------路径规划中，一条路径的拐角处改成圆弧路径,圆弧半径自适应-----------------------------------------------
    const double PI = 3.14159265358979323846;
    // distance 函数来计算两个点之间的距离，使用欧几里得距离公式来实现。
    double distance(geometry_msgs::PoseStamped p1, geometry_msgs::PoseStamped p2) {
        double dx = p1.pose.position.x - p2.pose.position.x;
        double dy = p1.pose.position.y - p2.pose.position.y;
        return sqrt(dx * dx + dy * dy);
    }

   // angle_between_vectors 函数来计算两个向量之间的夹角，使用向量点积和余弦定理来实现。
    double angle_between_vectors(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b, geometry_msgs::PoseStamped c) {
        //计算向量 ab  的坐标差值
        double ux = a.pose.position.x - b.pose.position.x;
        double uy = a.pose.position.y - b.pose.position.y;
        //计算向量 bc 的坐标差值
        double vx = c.pose.position.x - b.pose.position.x;
        double vy = c.pose.position.y - b.pose.position.y;
        //使用向量点积和余弦定理计算夹角的余弦值
        double dot_product = ux * vx + uy * vy;
        double cos_theta = dot_product / (distance(a, b) * distance(c, b));
        //函数使用反余弦函数 acos() 计算夹角的弧度值，并返回结果。
        return acos(cos_theta);
    }
    //angle_between_points 函数来计算两个向量之间的夹角
    double angle_between_points(geometry_msgs::PoseStamped a, geometry_msgs::PoseStamped b, geometry_msgs::PoseStamped c) {
        double theta = angle_between_vectors(a, b, c);
        //如果点 b 的 y 坐标大于点 a 的 y 坐标，表示角度位于第二或第三象限，需要将夹角加上一个完整的圆周角（2 * PI）来调整角度的范围。
        if (b.pose.position.y > a.pose.position.y) {
            theta = 2 * PI - theta;
        }
       // 最后，函数返回计算得到的角度值。
        return theta;
    }


    //end---------------------------------------------
    //-start-----------------------------------------------
    class OnePlanToMap {//封装一条路径【储存像素】，包括起点，终点，路径点，路径长度
    public:
        //todo 1.起始点坐标世界坐标转像素坐标 [需要计算点数及t值]
          int world_i_start_x, world_i_start_y;//起始点坐标（像素）
        //todo 2.目标坐标 （像素）
          int world_i_goal_x, world_i_goal_y;
        // 两点之间的线段长度（像素）
        int  distance = 0;
        //todo ************************** 一阶贝塞尔曲线计算插值 *********** ========================================================*****************
        vector<Point> distance_plan;//路径点坐标（像素）[一阶贝塞尔曲线计算出来的两点的直线像素点坐标数据]
        OnePlanToMap() {}

        OnePlanToMap(unsigned int &world_i_start_x,unsigned int &world_i_start_y, unsigned int &world_i_goal_x,unsigned int &world_i_goal_y,int  &distance ,vector<Point> &distance_plan) {
            this->world_i_start_x = (int)world_i_start_x;
            this->world_i_start_y = (int)world_i_start_y;
            this->world_i_goal_x = (int)world_i_goal_x;
            this->world_i_goal_y = (int)world_i_goal_y;
            this->distance = distance;
            this->distance_plan = distance_plan;

        }
    };


    //-end--------------------------------------


    //-start-----------------------------------------------
    class MorPlanToMap {//封装多条路径【储存像素】，包括起点，终点，路径点，路径长度
    public:

        string plan_type;//类型
        int plan_id;;//当前路径id
        string plan_name;//当前路径name
        string ns;//当前路径 namespace
        std::vector<int> target_plan_ids;//当前路径行走结束后，可以到达的路径id
        vector<Point>  distance_plan_all_vector;//todo 保存所有路径点 像素坐标
        std::vector<int> plan_indexs;//当前路径在地图中的索引，//当前路径在地图中的索引,在行优先的一维地图中的索引
        std::vector<geometry_msgs::PoseStamped> current_plan;//当前路径，坐标直 world世界坐标
        int cost = POT_HIGH;//当前路径的代价,初始值无穷大
       MorPlanToMap() {}

        MorPlanToMap(string &plan_type,int &plan_id,string &plan_name, std::vector<int> &target_plan_ids,std::vector<geometry_msgs::PoseStamped> &current_plan,vector<Point>  &distance_plan_all_vector,std::vector<int> &plan_indexs) {
            this->plan_id =plan_id;//路径唯一id
            this->plan_name = plan_name;//路径名称
            this->target_plan_ids = target_plan_ids;//当前路径行走结束后，可以到达的路径id
            this->current_plan = current_plan;////规划好的当前路径，坐标直 world世界坐标
            this->plan_type = plan_type;//路径类型
            this->distance_plan_all_vector = distance_plan_all_vector;
            this->plan_indexs = plan_indexs;////当前路径在地图中的索引,在行优先的一维地图中的索引

        }
        MorPlanToMap(int &plan_id,string &plan_name, std::vector<int> &target_plan_ids,std::vector<geometry_msgs::PoseStamped> &current_plan,vector<Point>  &distance_plan_all_vector,std::vector<int> &plan_indexs) {
            this->plan_id =plan_id;//路径唯一id
            this->plan_name = plan_name;//路径名称
            this->target_plan_ids = target_plan_ids;//当前路径行走结束后，可以到达的路径id
            this->current_plan = current_plan;////规划好的当前路径，坐标直 world世界坐标
            this->distance_plan_all_vector = distance_plan_all_vector;
            this->plan_indexs = plan_indexs;////当前路径在地图中的索引,在行优先的一维地图中的索引

        }

        MorPlanToMap(string &plan_type,int &plan_id,string &plan_name, std::vector<int> &target_plan_ids,vector<Point>  &distance_plan_all_vector,std::vector<int> &plan_indexs) {
            this->plan_id =plan_id;//路径唯一id
            this->plan_name = plan_name;//路径名称
            this->target_plan_ids = target_plan_ids;
            this->plan_type = plan_type;//路径类型
            this->distance_plan_all_vector = distance_plan_all_vector;
            this->plan_indexs = plan_indexs;////当前路径在地图中的索引,在行优先的一维地图中的索引

        }
        //反转数据
        std::vector<int> reverseData(map<int, MorPlanToMap> &execute_map_plan) {
            // 倒序排序 distance_plan_all_vector, plan_indexs, current_plan
            std::reverse(distance_plan_all_vector.begin(), distance_plan_all_vector.end());
            std::reverse(plan_indexs.begin(), plan_indexs.end());
            std::reverse(current_plan.begin(), current_plan.end());

            // 重新组合 target_plan_ids
            std::vector<int> new_target_plan_ids;
            for (auto &other_map_plan : execute_map_plan) {
              //  if (other_map_plan.first != this->plan_id) { //todo bug，当只有1条路径且首尾相链接时，会出现bug，把这行注释掉试试
                    for (int other_target_id : other_map_plan.second.target_plan_ids) {
                        if (other_target_id == this->plan_id) {
                            new_target_plan_ids.push_back(other_map_plan.second.plan_id);
                            break;
                        }
                    }
                }
          //  }
            return new_target_plan_ids;
        }


    };


    //-end--------------------------------------
    //曼哈顿距离封装
    class PointDistance {
    public:
        PointDistance(){};
        //index ：每个点在地图中的索引，start_distance：起点到当前路径所有点的曼哈顿距离,goal_distance：终点点到当前路径所有点的曼哈顿距离    ，point：当前点位终点最近的点，plan_id 路径id，plan_name路径名称，ns路径的命名空间
        PointDistance(int &index,int &distance_plan_all_point_index, double &start_distance,double &goal_distance,Point &point,int &plan_id,string &plan_name,string &ns) {
            this->index = index; //每个点在行有优先的地图中的索引
            this->start_distance = start_distance;////计算起点到当前路径所有点的曼哈顿距离
            this->goal_distance=goal_distance;///计算终点点到当前路径所有点的曼哈顿距离
            this->point=point;//当前点位最近的点
            this->distance_plan_all_point_index=distance_plan_all_point_index;//当前点在distance_plan_all_point, vector集合中的索引

            this->plan_id=plan_id; //路径id
            this->plan_name=plan_name;//plan_name路径名称
            this->ns=ns;//ns路径的命名空间

        }

        int  distance_plan_all_point_index;// 点在distance_plan_all_point, vector集合中的索引

        int index;//每个点在行有优先的地图中的索引
        double start_distance;//计算起点到当前路径所有点的曼哈顿距离。
        double goal_distance  ;//计算终点到当前路径所有点的曼哈顿距离。
        Point point; //当前点位终点最近的点 像素
        int plan_id;//当前路径id
        string plan_name;//当前路径name
        string ns;//当前路径 namespace

    };
    //-end--------------------------------------

    //todo -start-----------------------------------------------
    //储存点到点规划的路径，记录key：终点路径的路径索引id 对应的 规划的路径封装的数据结构
    class CurrentPathGoToTargetPath {
    public:
        CurrentPathGoToTargetPath(){};

        CurrentPathGoToTargetPath(std::vector<Point> &points,std::vector<geometry_msgs::PoseStamped> &plan) {
            this->plan_points=points;
            this->plan=plan;

        }

        std::vector<Point> plan_points;
        std::vector<geometry_msgs::PoseStamped> plan;

    };
    //-end--------------------------------------
    class global_planner {
    public:
        global_planner(tf2_ros::Buffer &tf,ros::NodeHandle &nh);

        ~global_planner();

        //多线程 处理状态机
        void thread_msg();

        //多线程 处理状态机
       // void stopCmd_thread();

        //刷新数据
        void callBack_refreshdb(std_msgs::String data);

        //定时执行
        void updatefun(const ros::TimerEvent &time_event);

        // sql 查询字典数据
     //   void queryAllFromTABLE_DICTIONARES();

        //日志级别 日志内容
        bool saveLogFormat(string logLevel, string className, int classLine, char *format, ...);

//sql where 条件
//        void whereSql(std::string Condition, std::string Key_i, std::string Symbol, std::string Value,
//                      contnav_srvs::TABLE_CONTORLLER &tableContorllerService);

//sql 封装条件数据
//        void mapTOcontnav_srvs(map <string, string> &dictionarieMap_request_value,
//                               contnav_srvs::TABLE_CONTORLLER &tableContorllerService);

        bool callback_global_plannerFun(contnav_srvs::global_planner::Request &request,
                                        contnav_srvs::global_planner::Response &response);

        void init();

        void run();

    private:
        string local_plan_toptic_="/move_base_flex/TebLocalPlannerROS/local_plan";//局部路径tooic名称


        NavigationStateMachine state_machine_;//导航状态机

        tf2_ros::Buffer &tf_;

        int nx_, ny_, ns_; /**< size of grid, in pixels */
        mutex m_;
        ros::NodeHandle nh_;
        std::string map_frame_;
        //因为 move_base_flex中没有 move_base_simple/goal toptic，所以在这里需要制作一个与movebase中一致可用的 move_base_simple/goal
        ros::Publisher action_goal_pub_,goal_pub_;
        ros::Subscriber sub_, goal_sub_;

        //rviz中获取点击的点坐标
        ros::Subscriber waypoint_sub_;

        //todo 全局地图
        ros::Subscriber costmap_sub_;
        ros::Subscriber costmap_updated_sub_;

        //路径发布,带圆弧路径发布
        ros::Publisher plan_pub_, plan_arc_pub_, pathPlan_semiStructured_pub_;
        //多路径发布
        ros::Publisher marker_pub_, marker_arc_pub_, marker_connect_pub_;
        nav_msgs::OccupancyGrid grid_;
        std::atomic_bool grid_ready_;
        std::atomic_bool grid_updated_ready_;//更新一个全局大地图
        std::shared_ptr<costmap_2d::Costmap2D> origin_costmap_;
        std::string frame_id_;
        unsigned int cx_, cy_;
        tf2_ros::Buffer buffer2;

        //todo 需要执行的多个路径  [通过这里的数据来实现半结构化路径规划]
        map<int, MorPlanToMap> execute_map_plan_;//需要执行的多个路径
        //翻转
        map<int, MorPlanToMap> execute_map_plan_Overturn_;//需要执行的多个路径

        std::vector<int> plan_all_indexs_;//当前所有半机构化网格路径在地图中的所有索引
        //当前路径到达目标路径的路径；储存点到点规划的路径，记录key：起始路径的路径索引id 和 value: 终点路径的路径索引id 与规划的路径封装的数据结构
        map<int, map<int, CurrentPathGoToTargetPath>> current_path_goTo_target_path_map_plan_;//
        //翻转路径
        map<int, map<int, CurrentPathGoToTargetPath>> current_path_goTo_target_path_map_plan_Overturn_;//
        //找到起始点与结束点的最近切入点 超过距离阀值
        double start_distance_threshold_ = 2;//切入点最小距离阀值 单位m
        double goal_distance_threshold_ = 1;//切出点最小距离阀值 单位m

        double startpose_distance_threshold_ = 5;////切入点最小距离阀值， 判断起始点,判断终点是否在路径上 判断起始点,判断终点是否在路径上.小于这个阀值就认为在路径上
        double goalpose_distance_threshold_ = 5;//切出点最小距离阀值 。判断终点是否在路径上 判断起始点,判断终点是否在路径上，小于这个阀值就认为在路径上

        // true:选择最近点作为切入和切出点（切入点与切出点相对固定，取决于起点和终点的位置【满足 切入点最小距离阀值，切出点最小距离阀值的范围内选择】）， false：选择组合路径距离最短的点作为切入和切出点（满足  切入点最小距离阀值，切出点最小距离阀值的范围内选择）
        bool isthresholdClosestPoint_ = false;
        //是否保存当前计算出来的路径到  fileOutPut/planpose/plan.txt
        bool isSavePlanToTxt_ = false;
        //get some parameters that will be global to the move base node
        std::string global_planner_name_, local_planner_name_;
        bool initialized_, allow_unknown_;
        //    1451    private_nh2.param("default_tolerance", default_tolerance_, 0.0);//#默认0.0，目标容差
        double planner_window_x_, planner_window_y_, default_tolerance_;
        int publish_scale_;
        bool outline_map_;
        string robot_base_frame_ = "base_link";//机器人底盘坐标系
        int markerId = 0;//多路径发布 markerId
        bool publish_potential_ = false;//true 发布代价地图
        unsigned char lethal_cost_ = 253;
        unsigned char neutral_cost_ = 66;
        bool unknown_ = false;//是否探索未知区域，flase--不可到达
        //日志 ,恢复代价地图
        ros::ServiceClient contnavLoggerClient,clear_costmaps_client_;
        //数据库操作
       // ros::ServiceClient SqlOperationClient;
        ros::ServiceServer global_plannerServer;
        //刷新数据库,订阅局部路径规划
        ros::Subscriber subscriber_refreshdb,local_path_sub_;
        ros::Timer timer2_;
        // //发布代价地图
        ros::Publisher potential_pub_;
        //字典数据
        map <string, string> dictionariesMap_;
        string logFileName = "global_planner.log";
        Index *open_set_array_;
        std::vector<Index> open_set_; //待遍历的点
        std::vector<Index> queue_;
        //这段代码按照行优先的顺序迭代potential_数组的每个元素，并使用std::cout输出其值。表达式i * ny + j计算了在展开的数组中(i, j)元素的索引。每行末尾的std::endl确保输出按照每行一个的方式打印。
        //需要注意的是，由于potential_数组有384 x 384 = 147,456个元素，因此输出可能非常大。你可以将输出重定向到文件中，或者将其管道传递给分页程序（例如less），以便更轻松地查看。
        float *potential_; //地图数组存代价直

        PotentialCalculator *p_calc_;
        Expander *planner_ = NULL;
        boost::mutex mutex_;//自动解锁
        boost::mutex mutex1_;//自动解锁
        boost::mutex mutex2_;//自动解锁

        //使用： 自动解锁
        Mutex m_mutex;
        //存储 rviz 点的坐标信息
        vector <geometry_msgs::PoseStamped> waypoints_;
        geometry_msgs::PoseStamped target_pose_default_;//返回默认点位，或是充电点位
        //1.封装一条路径【储存像素】，包括起点，终点，路径点，路径长度  [计算两条线段交点处的圆弧路径使用]
        vector <OnePlanToMap> onePlanToMaps_vector_;
        int point_size_t_ = 30;// 默认10 绘制点数量，决定圆弧的大小 （多条路径点位数量 大于point_min_size_t_*2 或者只有2条直线时路径点位数量大于point_min_size_t_*1 绘制圆弧）
        int point_min_size_t_ = 10;//默认10  绘制点数量，决定圆弧的大小[对于短路径（路径点位数量小于point_min_size_t_ 为短路径），决定圆弧的大小] ，point_size_t_数据应当大于point_min_size_t_的数据
        bool isOneWay_ =false; //  是否单向导航 true 单向导航，false 双向导航
        //参数服务器
        boost::recursive_mutex configuration_mutex_;
        dynamic_reconfigure::Server<contnav_global_planner::ContnavGlobalPlannerConfig> *dsrv_;

        void reconfigureCB(contnav_global_planner::ContnavGlobalPlannerConfig &config, uint32_t level);

        string move_base_global_toptic_ = "/move_base_flex/global_costmap/costmap";// 订阅全局地图   /move_base_flex/global_costmap/costmap  /move_base/global_costmap/costmap
        string move_base_global_updates_toptic_ = "/move_base_flex/global_costmap/costmap_updates";//    /move_base_flex/global_costmap/costmap_updates  /move_base/global_costmap/costmap_updates
        string manyPath_toptic_ = "/manyPath";//多路径发布  visualization_msgs::Marker
        string manyPath_arc_toptic_ = "/manyPathArc";//带圆弧的多路径发布  visualization_msgs::Marker
        string manyPath_connect_toptic_ = "/manyConnectArc";//上一个路径与下一个路径的链接路径发布  visualization_msgs::Marker

        string path_toptic_ = "/plan";//path one单路径发布
        string path_arc_toptic_ = "/planArc";//path 单路径发布 带圆弧
        string pathPlan_semiStructured_toptic_ = "/planSemiStructured";//半结构规划路径发布

        string clicked_point_toptic_ = "/clicked_point";//rviz中获取点击的点坐标
        string fileOutPut_ = "/home/sukai/workspace/workspace_ros_car_noetic/src/contnav_global_planner/output";//可配置路径




        OrientationFilter *orientation_filter_;//计算方向

    private://feedbackCb中增加其它功能：1.检查机器人是否卡住。2.当机器人到达特定位置时执行其他操作（例如发布一条消息）。
        geometry_msgs::PoseStamped target_pose_; // 目标位置
        geometry_msgs::PoseStamped last_pose_; // 上一次的位置

        void initialize();

        //订阅一个全局大地图，做障碍和珊格地图的提取
        void grid_callback(nav_msgs::OccupancyGridConstPtr const &msg);

        void grid_updated_callback(map_msgs::OccupancyGridUpdateConstPtr const &msg);

        auto grid2costmap(nav_msgs::OccupancyGrid const &grid) const -> std::shared_ptr<costmap_2d::Costmap2D>;

        // auto costmap2cv_mat(std::shared_ptr<costmap_2d::Costmap2D> const& costmap,int32_t x,int32_t y,int32_t width,int32_t height) -> cv::Mat;

        //清除成本图中的起始单元格，因为我们知道它不会成为障碍
        void clearRobotCell(const geometry_msgs::PoseStamped &global_pose, unsigned int mx, unsigned int my);

        /**
     * 具体而言，该函数的实现方式是，分别用指针 pc 指向数组的第一行、最后一行、第一列和最后一列，
     * 然后使用循环依次遍历这四个边界，并将它们的每个元素设置为指定的 value 值。

    在代码的实现过程中，nx 和 ny 分别表示数组 costarr 的宽度和高度，unsigned char 是一个无符号字符类型，用
     于存储 8 位无符号整数。因此，该函数的输入参数包括一个指向无符号字符数组的指针 costarr，数组的宽度 nx 和高度 ny，以及要设置的值 value。
     * @param costarr
     * @param nx
     * @param ny
     * @param value
     */
        void outlineMap(unsigned char *costarr, int nx, int ny, unsigned char value);


        //计算起点在一维数组中的下标。// 将起点坐标 start_x 和 start_y 转化为一维数组中的索引 start_i。
        int toIndex(int x, int y, int nx, int ny);

        //某个位置在字符数组中的索引为 index，则该位置在地图中的 current_x 和 current_y 像素坐标可以通过以下公式计算：
        void toX_Y(int index, int &current_x, int &current_y, int nx, int ny);

        void setSize(int nx, int ny);

//获取小车当前位置
        bool getRobotPose(geometry_msgs::PoseStamped &global_pose);

        bool getuwb_tf(string &pframe_id, string &cframe_id, geometry_msgs::TransformStamped &ps_out);

        bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                      std::vector<geometry_msgs::PoseStamped> &plan);

        bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                      double tolerance, std::vector<geometry_msgs::PoseStamped> &plan);

        void add(unsigned char *costs, float *potential, int next_i, int current_i, int start_x, int start_y, int end_x,
                 int end_y);

        //sukai 从可行点中提取路径，从potential 代价数组中提取路径；//start_x, start_y, goal_x, goal_y 像素坐标；//path 路径点集合
        bool getPath(float *potential, int start_x, int start_y, int end_x, int end_y,
                     std::vector<std::pair<int, int> > &path);

        bool getPlanFromPotential(float *potential, int start_x, int start_y, int goal_x, int goal_y,
                                  const geometry_msgs::PoseStamped &goal,
                                  std::vector<geometry_msgs::PoseStamped> &plan);

        //计算距离起点的对角距离
        int BaseCost(int current_x, int current_y, int start_x, int start_y);
/**
    这段代码是一个名为MyGlobalPlannerPlugin的插件中的函数，函数名为BaseCost。这个函数计算从起点到当前点的代价，代价的计算方式是通过曼哈顿距离和对角线距离的加权和来实现的。
    具体来说，这个函数的参数包括当前点的x坐标和y坐标，以及起点的x坐标和y坐标。函数首先计算当前点到起点的横向距离（x_dis）和纵向距离（y_dis），然后将这两个距离相加，得到曼哈顿距离。
    接下来，函数使用对角线距离来计算当前点到起点的斜向距离，这里使用了一个公式：(sqrt(2) - 2) * std::min(x_dis, y_dis)。其中sqrt(2)是根号2的值，std::min(x_dis, y_dis)表示取x_dis和y_dis中的最小值。
    这个公式的含义是，如果当前点和起点之间的距离是斜向的，那么就用这个公式来计算斜向距离。这个公式的实际作用是计算斜向距离和曼哈顿距离之间的差距，以便更加准确地估算当前点到起点的距离。
    最后，函数将曼哈顿距离和斜向距离加权求和，得到从起点到当前点的代价，并将这个代价作为函数的返回值。
*/
        //计算距离终点的对角距离
        int HeuristicCost(int current_x, int current_y, int end_x, int end_y);

        //总的对角距离
        int TotalDistance(int current_x, int current_y, int start_x, int start_y, int end_x, int end_y);

        //计算当前点到终点的曼哈顿距离
        float manhattanDistanceHeuristicCost(int &current_x, int &current_y, int end_x, int end_y);

        //像素坐标转世界坐标
        void mapToWorld(double mx, double my, double &wx, double &wy);

/**
 *
 * @param wx
 * @param wy
 * @param mx
 * @param my
 这个函数的实现比较简单，主要包括以下几个步骤：

    检查给定的世界坐标 (wx, wy) 是否在 Costmap2D 对象覆盖的区域内，如果不在，则返回 false。

    将给定的世界坐标 (wx, wy) 转换为相应的离散地图坐标 (mx, my)。

    检查转换后的地图坐标 (mx, my) 是否在 Costmap2D 对象的有效范围内，如果超出，则返回 false。

    如果以上检查都通过了，则返回 true。

下面是对这个函数中各个参数的说明：

    wx 和 wy：待转换的世界坐标。
    mx 和 my：转换后的离散地图坐标。像素坐标
    origin_x_ 和 origin_y_：地图坐标系原点在世界坐标系中的位置。
    resolution_：地图的分辨率，即一个像素代表的实际距离。
    size_x_ 和 size_y_：地图的大小，即像素数。

需要注意的是，以上实现中使用了 C++ 中的类型转换语句 static_cast，将浮点数转换为整数类型，同时使用引用类型 & 将结果返回给调用者。
 */

        bool worldToMap(double wx, double wy, double &mx, double &my);

        //出于可视化目的发布计划
        void publishPlan(const std::vector<geometry_msgs::PoseStamped> &path, nav_msgs::Path &gui_path);

        //出于可视化目的发布计划 MarkerPlan ，int rgb 颜色 1：红，2：绿，3 蓝， ,doubl scale  0.1大小
        void publishMarkerPlan(visualization_msgs::Marker &marker, const std::vector<geometry_msgs::PoseStamped> &path,
                               int rgb, double scale);

        //设置marker类型
        void setMarkerType(uint32_t &shape);

        /**
* 这个函数是一个全局路径规划器的类中的一个函数，用于发布潜在代价地图。函数的输入是一个浮点型数组，表示每个点到目标节点的距离。函数的输出是一个nav_msgs::OccupancyGrid类型的消息，表示整个代价地图。
* 函数首先获取代价地图的大小和分辨率，
* 然后将整个代价地图发布出去。在发布代价地图之前，函数会对代价地图进行处理，将代价地图中的每个点的值进行归一化处理，使得代价地图中的最大值为1，最小值为0。最后，函数将处理后的代价地图发布出去。
* @param potential
*/
        void publishPotential(float *potential);

        //设置 geometry_msgs::PoseStamped数据保存到plan路径中
        void setPoseStampedPoseToPlan(geometry_msgs::PoseStamped &pose, double &x, double &y,
                                      std::vector<geometry_msgs::PoseStamped> &plan);

        //一阶 Function to return point on Linear Bezier Curve
        //ps:起始点   pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
        vector<double> FirstBezier(vector<double> ps, vector<double> pe, double t);

        //二阶 Function to return point on Quadratic Bezier Curve
        //ps:起始点 pc:控制点  pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
        vector<double> QuadraticBezier(vector<double> ps, vector<double> pc, vector<double> pe, double t);

        //三阶 Function to return point on Cubic Bezier Curve
        //ps:起始点 pc1:控制点1  pc2:控制点2 pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
        vector<double>
        CubicBezier(vector<double> ps, vector<double> pc1, vector<double> pc2, vector<double> pe, double t);

        //四阶 Function to return point on Equation Bezier Curve
        //ps:起始点 pc1:控制点1  pc2:控制点2 pc3:控制点3 pe:结束点  t: 0-1之间的数据；t:   绘制40个点，i从0开始计数到最大值40，计算  t = i / 40.0
        vector<double>
        EquationBezier(vector<double> ps, vector<double> pc1, vector<double> pc2, vector<double> pc3, vector<double> pe,
                       double t);

        //---
        //通过起始点和结束点获取一条直线路径 startPoseStamped:起始点 endPoseStamped:结束点 plan:路径
        bool getline(geometry_msgs::PoseStamped &startPoseStamped, geometry_msgs::PoseStamped &endPoseStamped,
                     std::vector<geometry_msgs::PoseStamped> &plan, nav_msgs::Path &gui_path);

        // 获取两条直线的交点（把交点作为控制点） line1_p1:直线1的起始点 line1_p2:直线1的结束点 line2_p1:直线2的起始点 line2_p2:直线2的结束点 intersection:交点（像素坐标） pc1:控制点（世界坐标） 返回值：true:有交点 false:无交点 ；[输入点坐标为像素坐标]
        bool getIntersection(Point &line1_p1, Point &line1_p2, Point &line2_p1, Point &line2_p2, Point &intersection,
                             geometry_msgs::PoseStamped &pc1);

        //rviz 成功接收到了点的坐标信息
        void waypointCallback(const geometry_msgs::PointStampedConstPtr &waypoint);

        //geometry_msgs::PointStamped与geometry_msgs::PoseStamped互转
        geometry_msgs::PoseStamped pointStampedToPose(geometry_msgs::PointStamped &pointStamped);

        //计算路径的朝向，计算向量的斜率，向量的朝向
        tf2::Quaternion getSlopeOfVector(double sart_x, double sart_y, double end_x, double end_y, double &yaw);

        //  半结构化路径规划traffic_goal
        void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal);

        //设置 Point数据保存geometry_msgs::PoseStamped中
        void setPoseStampedPoseWithPoint(Point point, geometry_msgs::PoseStamped &pose);

        //todo 计算圆弧路径 ，1.返回 圆弧路径 plan 世界坐标。2.返回 圆弧路径 distance_plan_all_vector 像素坐标， 3.返回 带转角路径 distance_plan_noArc_vector 像素坐标 （转角未处理圆弧）；
        bool global_planner_path_with_rounded_corners(geometry_msgs::PoseStamped &startPoseStamped,
                                                      std::vector<geometry_msgs::PoseStamped> &plan,
                                                      vector <Point> &distance_plan_all_vector,
                                                      vector <Point> &distance_plan_noArc_vector);

        //todo 发布路径规划的marker，用于可视化
        bool publishPlanMarkerFun(visualization_msgs::Marker &marker, std::vector<geometry_msgs::PoseStamped> &plan,
                                  nav_msgs::Path &gui_path);


        //todo 判断起始点在不在结构化网格上
        bool is_index_in_planFun(int &index,std::vector<int> &plan_all_indexs);

        //todo 判断起始切入点是否在当前的路径上 global_planner::is_index_in_planFun(int &find_index,std::vector<int> &plan_all_indexs,int &result_i)
        bool is_index_in_planFun(int &index, std::vector<int> &plan_all_indexs, int &result_i);

        //todo 2.规划路径 半结构路径规划中的 makePlan2 函数
        bool makePlan2(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
                       double tolerance, std::vector<geometry_msgs::PoseStamped> &plan);

        //todo 2.规划路径 半结构路径规划中的 makePlan2Overturn 函数 翻转路径
        bool makePlan2Overturn(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
                                               double tolerance, std::vector<geometry_msgs::PoseStamped>& plan);

        //todo 规划路径，半结构路径规划中的 makePlan函数,algorithm使用那个规划算法
        bool makePlanWithSemiStructuredPath(Point &startpoint, Point &target_goal_point, string algorithm,
                                            double tolerance, std::vector<geometry_msgs::PoseStamped> &plan);


        //这里需要找到最近的结构化网格点
        bool find_nearest_index(unsigned char *costs, int &start_index, int &nearest_index);

        // todo 打印代价矩阵
        void print_graph(MorPlanToMap *morPlanToMap_potential);


        //todo *** 可以使用广度优先搜索（BFS）算法来找出起始点到目标点的所有路径，并计算每条路径的长度，然后选择路径长度最短的路径作为结果输出。 distance_start_map 切入点起点，distance_goal_map 切出点终点，map_plan 代价矩阵，all_paths_id 所有路径的id，result_message 结果信息
        bool find_shortest_path(map<int, PointDistance> distance_start_map, map<int, PointDistance> distance_goal_map,
                                std::map<int, MorPlanToMap> map_plan, std::vector<std::vector<int>> &all_paths_id,
                                string &result_message);

        bool makePlanWithSemiStructuredPath2(Point &start_point, Point &goal_point,
                                             std::vector<geometry_msgs::PoseStamped> &makePlanWithSemiStructuredPlan,
                                             std::vector<Point> &plan_points, string algorithm);

        //    PointDistance &start_min_pointDistance ： 起始切入点,PointDistance &goal_min_pointDistance：目标切切出点
        //获取半结构全局规划路径,path_id:路径的id,int start_min_index 起始切入路径索引, PointDistance &start_min_pointDistance ： 起始切入点,PointDistance &goal_min_pointDistance：目标切切出点，int goal_min_index:目标切切出点; Point &start_point 起点点位,Point &goal_point终点点位，//找到起始点的最近切入点-，找到 start_x start_y最近的路径对应的点位 Point start_min_point  ；//找到结束点的最近切入点-。找到 goal_x goal_y最近的路径对应的点位Point goal_min_point
        bool getShortestPath(std::vector<std::vector<int>> &all_paths, map<int, MorPlanToMap> &execute_map_plan,
                             map<int, PointDistance> distance_start_map, map<int, PointDistance> distance_goal_map,
                             Point &start_point, Point &goal_point, std::vector<geometry_msgs::PoseStamped> &plan,
                             string &result_message,map<int, map<int, CurrentPathGoToTargetPath>> current_path_goTo_target_path_map_plan);

        //添加方向
        void calculate_direction(geometry_msgs::PoseStamped const &pose1,
                                 geometry_msgs::PoseStamped &pose2);


        //写入文件
        void writeEulerAnglesToFile(const std::vector<geometry_msgs::PoseStamped> &plan, const std::string &filename);

        //创建文件路径
        void createDirectory(const std::string &path);

        //读取文件
        std::vector<geometry_msgs::PoseStamped> readEulerAnglesFromFile(const std::string &filename);
        //发布路径
        bool  sendGoalPlan(geometry_msgs::PoseStamped &robot_pose,geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan);
        //翻转路径
        bool   sendGoalPlanOverturn(geometry_msgs::PoseStamped &robot_pose, geometry_msgs::PoseStamped &goal,std::vector<geometry_msgs::PoseStamped> &plan);
        //接收机器人的当前位置，并将其与全局路径上的点进行比较。如果机器人当前位置与全局路径上的某个点的距离小于一个阈值（例如0.1米），我们认为机器人已经经过这个点。然后将这个点从未走过的路径中删除，并添加到已走过的路径中。
        void updatePaths( geometry_msgs::PoseStamped &current_pose);
        //订阅局部路径规划
        void localPathCallback(const nav_msgs::Path::ConstPtr& local_path);

         //状态机
        // 调用 MBF 客户端导航至目标点
        void navigate_to_goalpath(const std::vector<boost::any> &args);

        // 切换到监控状态,监控是否遇到障碍物，遇到就停止
      //  void monitor_progress(const std::vector<boost::any> &args);

        // 调用 MBF 客户端导航至目标点 (翻转路径)
        void navigate_to_goalpathOverturn(const std::vector<boost::any> &args);

        //给充电桩发送充电指令
        void chargeFun(const std::vector<boost::any> &args);
        //获取数据 boost::any 中的数据，函数的输入参数包括 boost::any 对象和其期望的类型，函数的返回值是类型转换后的值
        template<typename T> T any_cast(const boost::any & operand);

        //判断map判断key是否存在
        template<typename KeyType, typename ValueType>
        bool containsKey(const std::map<KeyType, ValueType>& myMap, const KeyType& keyToCheck);

        /**
        //start=================路径优化===========================
        // Helper function to compute the euclidean distance between two points 计算两点之间欧氏距离的辅助函数
        double  euclidean_distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2);
        // Function to remove redundant points in the path 删除路径中多余点的功能
        void sparsify_path(std::vector<geometry_msgs::PoseStamped> &path, double distance_threshold);

        // Function to smooth the path using a simple moving average 使用简单移动平均值平滑路径的函数
        void smooth_path(std::vector<geometry_msgs::PoseStamped> &path, int window_size);

        // Function to optimize the global path 用于优化全局路径的功能
        void optimize_global_path(std::vector<geometry_msgs::PoseStamped> &path, double distance_threshold, int window_size);
        */

        double euclidean_distance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) ;
        void insert_intermediate_points(std::vector<geometry_msgs::PoseStamped>& path, double max_distance) ;
        void smooth_path(std::vector<geometry_msgs::PoseStamped>& path, int window_size) ;
        void calculate_orientations(std::vector<geometry_msgs::PoseStamped>& path) ;
        //end=================路径优化===========================


        //----------------------
    };

}



#endif //CONTNAV_GLOBAL_PLANNER_GLOBAL_PLANNER_H
